
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc_interface.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdint.h>

#include "rc_channel.h"
#include <common/time/gp_time.h>
#include <parameter/param.h>

#include <fms.h>
#include <mavproxy/mavproxy.h>
#include <common/gp_math/gp_mathlib.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/
#define SWITCH_DEBOUNCE_TIME_MS  200
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static itc_node_t pNodeRcRaw = NULL;
static uitc_actuator_rc actuator_rc = {0};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       读取遥控器通道值
  * @param[in]   pRc  
  * @param[out]  
  * @retval      
  * @note        
  */
bool RC_update(RC_HandleTypeDef *pRc, int16_t ChanVal)
{
    RCs_HandleTypeDef *pRCs;
    pRCs = rc_container_of(pRc, RCs_HandleTypeDef, channel[pRc->ch_in]);

    if (RC_has_override(pRc) && !RCs_ignore_receiver(pRCs)) {
        pRc->radio_in = pRc->override_value;
    } else if (RCs_has_had_rc_receiver(pRCs) && !RCs_ignore_receiver(pRCs)) {
        pRc->radio_in = ChanVal;
    } else {
        return false;
    }

    if (pRc->type_in == RC_CHANNEL_TYPE_RANGE) {
        pRc->control_in = RC_pwm_to_range(pRc);
    } else {
        //RC_CHANNEL_TYPE_ANGLE
        pRc->control_in = RC_pwm_to_angle(pRc);
    }
    
    return true;
}

bool RC_has_override(RC_HandleTypeDef *pRc)
{
    RCs_HandleTypeDef *pRCs;
    pRCs = rc_container_of(pRc, RCs_HandleTypeDef, channel[pRc->ch_in]);
    
    if (pRc->override_value == 0) {
        return false;
    }

    uint32_t override_timeout_ms;
    if (!RCs_get_override_timeout_ms(pRCs, &override_timeout_ms)) {
        // timeouts are disabled
        return true;
    }

    if (override_timeout_ms == 0) {
        // overrides are explicitly disabled by a zero value
        return false;
    }

    return (time_millis() - pRc->last_override_time < override_timeout_ms);
}

/**
  * @brief       
  * @param[in]   pRc  
  * @param[in]   v  
  * @param[in]   timestamp_ms  
  * @param[out]  
  * @retval      
  * @note        
  */
void RC_set_override(RC_HandleTypeDef *pRc, const uint16_t v, const uint32_t timestamp_ms)
{
    RCs_HandleTypeDef *pRCs;
    pRCs = rc_container_of(pRc, RCs_HandleTypeDef, channel[pRc->ch_in]);

    if (!pRCs->gcs_overrides_enabled) {
        return;
    }

    pRc->last_override_time = timestamp_ms != 0 ? timestamp_ms : time_millis();
    pRc->override_value = v;
    pRCs->has_new_overrides = true;
}

// read_3pos_switch
bool RC_read_3pos_switch(RC_HandleTypeDef *pRc, RC_AuxSwitchPos *ret)
{
    RCs_HandleTypeDef *pRCs;
    pRCs = rc_container_of(pRc, RCs_HandleTypeDef, channel[pRc->ch_in]);

    const uint16_t in = pRc->radio_in;
    if (in <= 900 || in >= 2200) {
        return false;
    }
    
    // switch is reversed if 'reversed' option set on channel and switches reverse is allowed by RC_OPTIONS
    bool switch_reversed = pRc->_reversed && RCs_switch_reverse_allowed(pRCs);
    
    if (in < AUX_PWM_TRIGGER_LOW) {
        *ret = switch_reversed ? RC_HIGH : RC_LOW;
    } else if (in > AUX_PWM_TRIGGER_HIGH) {
        *ret = switch_reversed ? RC_LOW : RC_HIGH;
    } else {
        *ret = RC_MIDDLE;
    }
    return true;
}

// 
RC_AuxSwitchPos  RC_get_aux_switch_pos(RC_HandleTypeDef *pRc)
{
    RC_AuxSwitchPos position = RC_LOW;
    (void)RC_read_3pos_switch(pRc, &position);

    return position;
}

// 
bool RC_debounce_completed(RC_HandleTypeDef *pRc, int8_t position)
{
    // switch change not detected
    if (pRc->switch_state.current_position == position) {
        // reset debouncing
        pRc->switch_state.debounce_position = position;
    } else {
        // switch change detected
        const uint32_t tnow_ms = time_millis();

        // position not established yet
        if (pRc->switch_state.debounce_position != position) {
            pRc->switch_state.debounce_position = position;
            pRc->switch_state.last_edge_time_ms = tnow_ms;
        } else if (tnow_ms - pRc->switch_state.last_edge_time_ms >= SWITCH_DEBOUNCE_TIME_MS) {
            // position estabilished; debounce completed
            pRc->switch_state.current_position = position;
            return true;
        }
    }

    return false;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_init(RCs_HandleTypeDef *pRcs)
{
    pRcs->gcs_overrides_enabled = true;

    RCs_UpdateParam(pRcs);

    pRcs->_override_timeout = 3.0f;
    pRcs->_options          = RC_ARMING_CHECK_THROTTLE;
    pRcs->_protocols        = 1;

    // setup ch_in on channels
    for (uint8_t i=0; i<MAX_RC_CHANNEL_NUM; i++) {
        pRcs->channel[i].ch_in = i;
        RC_init(&(pRcs->channel[i]));
    }

    RCs_init_aux_all(pRcs);
}

/**
  * @brief       
  * @param[in]   
  * @param[out]  
  * @retval      
  * @note        
  */
bool RCs_read_input(RCs_HandleTypeDef *pRcs)
{
    if (pNodeRcRaw == NULL) {
        pNodeRcRaw = itc_subscribe(ITC_ID(vehicle_rc), NULL);
        return false;
    }

    
    if (itc_update(pNodeRcRaw)) {
        pRcs->has_had_rc_receiver = true;
        itc_copy(ITC_ID(vehicle_rc), pNodeRcRaw, &actuator_rc);
    } else if (!pRcs->has_new_overrides) {
        return false;
    }

    pRcs->has_new_overrides = false;

    pRcs->last_update_ms = time_millis();

    bool success = false;
    for (uint8_t i=0; i<MAX_RC_CHANNEL_NUM; i++) {
        success |= RC_update(&(pRcs->channel[i]), actuator_rc.channels[pRcs->channel[i].ch_in]);
    }

    return success;
}

/**
  * @brief       更新参数
  * @param[in]   pRcs  
  * @param[out]  
  * @retval      
  * @note        
  */
void RCs_UpdateParam(RCs_HandleTypeDef *pRcs)
{
    RC_HandleTypeDef *pRC = pRcs->channel;

    if (PARAM_GET_INT16(RC, RC1_MIN) != PARAM_GET_INT16(RC, RC_MIN))
    {
        PARAM_GET_INT16(RC, RC1_MIN) = PARAM_GET_INT16(RC, RC_MIN);
    }
    if (PARAM_GET_INT16(RC, RC1_MAX) != PARAM_GET_INT16(RC, RC_MAX))
    {
        PARAM_GET_INT16(RC, RC1_MAX) = PARAM_GET_INT16(RC, RC_MAX);
    }
    if (PARAM_GET_INT16(RC, RC1_TRIM) != PARAM_GET_INT16(RC, RC_MID))
    {
        PARAM_GET_INT16(RC, RC1_TRIM) = PARAM_GET_INT16(RC, RC_MID);
    }

    if (PARAM_GET_INT16(RC, RC2_MIN) != PARAM_GET_INT16(RC, RC_MIN))
    {
        PARAM_GET_INT16(RC, RC2_MIN) = PARAM_GET_INT16(RC, RC_MIN);
    }
    if (PARAM_GET_INT16(RC, RC2_MAX) != PARAM_GET_INT16(RC, RC_MAX))
    {
        PARAM_GET_INT16(RC, RC2_MAX) = PARAM_GET_INT16(RC, RC_MAX);
    }
    if (PARAM_GET_INT16(RC, RC2_TRIM) != PARAM_GET_INT16(RC, RC_MID))
    {
        PARAM_GET_INT16(RC, RC2_TRIM) = PARAM_GET_INT16(RC, RC_MID);
    }

    if (PARAM_GET_INT16(RC, RC3_MIN) != PARAM_GET_INT16(RC, RC_MIN))
    {
        PARAM_GET_INT16(RC, RC3_MIN) = PARAM_GET_INT16(RC, RC_MIN);
    }
    if (PARAM_GET_INT16(RC, RC3_MAX) != PARAM_GET_INT16(RC, RC_MAX))
    {
        PARAM_GET_INT16(RC, RC3_MAX) = PARAM_GET_INT16(RC, RC_MAX);
    }
    if (PARAM_GET_INT16(RC, RC3_TRIM) != PARAM_GET_INT16(RC, RC_MID))
    {
        PARAM_GET_INT16(RC, RC3_TRIM) = PARAM_GET_INT16(RC, RC_MID);
    }

    if (PARAM_GET_INT16(RC, RC4_MIN) != PARAM_GET_INT16(RC, RC_MIN))
    {
        PARAM_GET_INT16(RC, RC4_MIN) = PARAM_GET_INT16(RC, RC_MIN);
    }
    if (PARAM_GET_INT16(RC, RC4_MAX) != PARAM_GET_INT16(RC, RC_MAX))
    {
        PARAM_GET_INT16(RC, RC4_MAX) = PARAM_GET_INT16(RC, RC_MAX);
    }
    if (PARAM_GET_INT16(RC, RC4_TRIM) != PARAM_GET_INT16(RC, RC_MID))
    {
        PARAM_GET_INT16(RC, RC4_TRIM) = PARAM_GET_INT16(RC, RC_MID);
    }

    if (PARAM_GET_INT16(RC, RC5_MIN) != PARAM_GET_INT16(RC, RC_MIN))
    {
        PARAM_GET_INT16(RC, RC5_MIN) = PARAM_GET_INT16(RC, RC_MIN);
    }
    if (PARAM_GET_INT16(RC, RC5_MAX) != PARAM_GET_INT16(RC, RC_MAX))
    {
        PARAM_GET_INT16(RC, RC5_MAX) = PARAM_GET_INT16(RC, RC_MAX);
    }
    if (PARAM_GET_INT16(RC, RC5_TRIM) != PARAM_GET_INT16(RC, RC_MID))
    {
        PARAM_GET_INT16(RC, RC5_TRIM) = PARAM_GET_INT16(RC, RC_MID);
    }

    if (PARAM_GET_INT16(RC, RC6_MIN) != PARAM_GET_INT16(RC, RC_MIN))
    {
        PARAM_GET_INT16(RC, RC6_MIN) = PARAM_GET_INT16(RC, RC_MIN);
    }
    if (PARAM_GET_INT16(RC, RC6_MAX) != PARAM_GET_INT16(RC, RC_MAX))
    {
        PARAM_GET_INT16(RC, RC6_MAX) = PARAM_GET_INT16(RC, RC_MAX);
    }
    if (PARAM_GET_INT16(RC, RC6_TRIM) != PARAM_GET_INT16(RC, RC_MID))
    {
        PARAM_GET_INT16(RC, RC6_TRIM) = PARAM_GET_INT16(RC, RC_MID);
    }
    
    
    // 
    if (pRC[0]._radio_min != PARAM_GET_INT16(RC, RC1_MIN))
    {
        pRC[0]._radio_min = PARAM_GET_INT16(RC, RC1_MIN);
    }
    if (pRC[0]._radio_max != PARAM_GET_INT16(RC, RC1_MAX))
    {
        pRC[0]._radio_max = PARAM_GET_INT16(RC, RC1_MAX);
    }
    if (pRC[0]._radio_trim != PARAM_GET_INT16(RC, RC1_TRIM))
    {
        pRC[0]._radio_trim = PARAM_GET_INT16(RC, RC1_TRIM);
    }
    if (pRC[0]._dead_zone != PARAM_GET_INT16(RC, RC1_DZ))
    {
        pRC[0]._dead_zone = PARAM_GET_INT16(RC, RC1_DZ);
    }
    if (pRC[0]._option != PARAM_GET_INT16(RC, RC1_OPTION))
    {
        pRC[0]._option = PARAM_GET_INT16(RC, RC1_OPTION);
    }

    if (pRC[1]._radio_min != PARAM_GET_INT16(RC, RC2_MIN))
    {
        pRC[1]._radio_min = PARAM_GET_INT16(RC, RC2_MIN);
    }
    if (pRC[1]._radio_max != PARAM_GET_INT16(RC, RC2_MAX))
    {
        pRC[1]._radio_max = PARAM_GET_INT16(RC, RC2_MAX);
    }
    if (pRC[1]._radio_trim != PARAM_GET_INT16(RC, RC2_TRIM))
    {
        pRC[1]._radio_trim = PARAM_GET_INT16(RC, RC2_TRIM);
    }
    if (pRC[1]._dead_zone != PARAM_GET_INT16(RC, RC2_DZ))
    {
        pRC[1]._dead_zone = PARAM_GET_INT16(RC, RC2_DZ);
    }
    if (pRC[1]._option != PARAM_GET_INT16(RC, RC2_OPTION))
    {
        pRC[1]._option = PARAM_GET_INT16(RC, RC2_OPTION);
    }

    if (pRC[2]._radio_min != PARAM_GET_INT16(RC, RC3_MIN))
    {
        pRC[2]._radio_min = PARAM_GET_INT16(RC, RC3_MIN);
    }
    if (pRC[2]._radio_max != PARAM_GET_INT16(RC, RC3_MAX))
    {
        pRC[2]._radio_max = PARAM_GET_INT16(RC, RC3_MAX);
    }
    if (pRC[2]._radio_trim != PARAM_GET_INT16(RC, RC3_TRIM))
    {
        pRC[2]._radio_trim = PARAM_GET_INT16(RC, RC3_TRIM);
    }
    if (pRC[2]._dead_zone != PARAM_GET_INT16(RC, RC3_DZ))
    {
        pRC[2]._dead_zone = PARAM_GET_INT16(RC, RC3_DZ);
    }
    if (pRC[2]._option != PARAM_GET_INT16(RC, RC3_OPTION))
    {
        pRC[2]._option = PARAM_GET_INT16(RC, RC3_OPTION);
    }

    if (pRC[3]._radio_min != PARAM_GET_INT16(RC, RC4_MIN))
    {
        pRC[3]._radio_min = PARAM_GET_INT16(RC, RC4_MIN);
    }
    if (pRC[3]._radio_max != PARAM_GET_INT16(RC, RC4_MAX))
    {
        pRC[3]._radio_max = PARAM_GET_INT16(RC, RC4_MAX);
    }
    if (pRC[3]._radio_trim != PARAM_GET_INT16(RC, RC4_TRIM))
    {
        pRC[3]._radio_trim = PARAM_GET_INT16(RC, RC4_TRIM);
    }
    if (pRC[3]._dead_zone != PARAM_GET_INT16(RC, RC4_DZ))
    {
        pRC[3]._dead_zone = PARAM_GET_INT16(RC, RC4_DZ);
    }
    if (pRC[3]._option != PARAM_GET_INT16(RC, RC4_OPTION))
    {
        pRC[3]._option = PARAM_GET_INT16(RC, RC4_OPTION);
    }

    if (pRC[4]._radio_min != PARAM_GET_INT16(RC, RC5_MIN))
    {
        pRC[4]._radio_min = PARAM_GET_INT16(RC, RC5_MIN);
    }
    if (pRC[4]._radio_max != PARAM_GET_INT16(RC, RC5_MAX))
    {
        pRC[4]._radio_max = PARAM_GET_INT16(RC, RC5_MAX);
    }
    if (pRC[4]._radio_trim != PARAM_GET_INT16(RC, RC5_TRIM))
    {
        pRC[4]._radio_trim = PARAM_GET_INT16(RC, RC5_TRIM);
    }
    if (pRC[4]._dead_zone != PARAM_GET_INT16(RC, RC5_DZ))
    {
        pRC[4]._dead_zone = PARAM_GET_INT16(RC, RC5_DZ);
    }
    if (pRC[4]._option != PARAM_GET_INT16(RC, RC5_OPTION))
    {
        pRC[4]._option = PARAM_GET_INT16(RC, RC5_OPTION);
    }

    if (pRC[5]._radio_min != PARAM_GET_INT16(RC, RC6_MIN))
    {
        pRC[5]._radio_min = PARAM_GET_INT16(RC, RC6_MIN);
    }
    if (pRC[5]._radio_max != PARAM_GET_INT16(RC, RC6_MAX))
    {
        pRC[5]._radio_max = PARAM_GET_INT16(RC, RC6_MAX);
    }
    if (pRC[5]._radio_trim != PARAM_GET_INT16(RC, RC6_TRIM))
    {
        pRC[5]._radio_trim = PARAM_GET_INT16(RC, RC6_TRIM);
    }
    if (pRC[5]._dead_zone != PARAM_GET_INT16(RC, RC6_DZ))
    {
        pRC[5]._dead_zone = PARAM_GET_INT16(RC, RC6_DZ);
    }
    if (pRC[5]._option != PARAM_GET_INT16(RC, RC6_OPTION))
    {
        pRC[5]._option = PARAM_GET_INT16(RC, RC6_OPTION);
    }

    if (pRC[6]._radio_min != PARAM_GET_INT16(RC, RC7_MIN))
    {
        pRC[6]._radio_min = PARAM_GET_INT16(RC, RC7_MIN);
    }
    if (pRC[6]._radio_max != PARAM_GET_INT16(RC, RC7_MAX))
    {
        pRC[6]._radio_max = PARAM_GET_INT16(RC, RC7_MAX);
    }
    if (pRC[6]._radio_trim != PARAM_GET_INT16(RC, RC7_TRIM))
    {
        pRC[6]._radio_trim = PARAM_GET_INT16(RC, RC7_TRIM);
    }
    if (pRC[6]._dead_zone != PARAM_GET_INT16(RC, RC7_DZ))
    {
        pRC[6]._dead_zone = PARAM_GET_INT16(RC, RC7_DZ);
    }
    if (pRC[6]._option != PARAM_GET_INT16(RC, RC7_OPTION))
    {
        pRC[6]._option = PARAM_GET_INT16(RC, RC7_OPTION);
    }

    if (pRC[7]._radio_min != PARAM_GET_INT16(RC, RC8_MIN))
    {
        pRC[7]._radio_min = PARAM_GET_INT16(RC, RC8_MIN);
    }
    if (pRC[7]._radio_max != PARAM_GET_INT16(RC, RC8_MAX))
    {
        pRC[7]._radio_max = PARAM_GET_INT16(RC, RC8_MAX);
    }
    if (pRC[7]._radio_trim != PARAM_GET_INT16(RC, RC8_TRIM))
    {
        pRC[7]._radio_trim = PARAM_GET_INT16(RC, RC8_TRIM);
    }
    if (pRC[7]._dead_zone != PARAM_GET_INT16(RC, RC8_DZ))
    {
        pRC[7]._dead_zone = PARAM_GET_INT16(RC, RC8_DZ);
    }
    if (pRC[7]._option != PARAM_GET_INT16(RC, RC8_OPTION))
    {
        pRC[7]._option = PARAM_GET_INT16(RC, RC8_OPTION);
    }
}

/**
  * @brief       返回切换飞行模式的通道
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
uint8_t RCs_flight_mode_channel_number()
{
    return PARAM_GET_UINT8(FLIGHTMODE, FLTMODE_CH);
}

//
bool RCs_has_valid_input()
{
    if (fms.failsafe.radio) {
        return false;
    }
    if (fms.failsafe.radio_counter != 0) {
        return false;
    }
    return true;
}

uint8_t RCs_get_valid_channel_count(void)
{
    return MIN(MAX_RC_CHANNEL_NUM, actuator_rc.channel_count);
}

/*------------------------------------test------------------------------------*/


